////*****************************************************************************
////
////Stoch2.0
////Author: Ashish Joglekar, RBCCPS, IISc
////Features:
////x,y bound check
////abduction based turning
////timer interrupt based frequency control for gaits
////Data acquisition
////Standing sequence
////Calibration routine (disabled)
////ICS protocol custom implementation
////Interrupt driven UART RX
////IK Solver
////Stomping in place
////*****************************************************************************
//
//#include <stdint.h>
//#include <stdbool.h>
//#include "inc/hw_ints.h"
//#include "inc/hw_memmap.h"
////#include "inc/hw_types.h"
//#include "driverlib/debug.h"
//#include "driverlib/fpu.h"
//#include "driverlib/gpio.h"
//#include "driverlib/interrupt.h"
//#include "driverlib/sysctl.h"
////#include "driverlib/uart.h"
//#include "driverlib/rom.h"
//#include "grlib/grlib.h"
//#include "inc/tm4c123gh6pm.h"
//#include "inc/hw_memmap.h"
//#include "driverlib/adc.h"
//#include "driverlib/pin_map.h"
//#include "driverlib/sysctl.h"
////#include "driverlib/uart.h"
//#include "Peripherals/uart.h"
//#include "driverlib/pwm.h"
//#include "driverlib/timer.h"
//#include "inc/hw_nvic.h"
//#include "inc/hw_types.h"
//#include <math.h>
//#include "driverlib/rom_map.h"
//#include "utils/uartstdio.h"
//#include "bezier.h"
//#define PI 3.14159
//
//volatile long motor_counter=0;
////volatile uint32_t ui32Load;
////volatile uint32_t ui32PWMClock;
////volatile int phase[]={0,50,50,0};
//volatile char rxchar[10];
//volatile float valuerx=0;
//volatile int startrx=0,startrx2=0;
//const float slope = (8000/270);
//volatile float q1,q2,q3,q4;
//volatile int motor_hip[]={0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int motor_knee[]={0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int motor_abd[]={0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int actpos[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int current[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int velocity[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int speed[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile int temperature[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//volatile float new_period=5;
////volatile float reference[]={0,4250,11470,6100,3813,11468,6100,3912,11493,6100,3880,11017,6100};
////volatile float reference[]={0,4250,11470,6100,3949,11386,6100,3912,11493,6100,4330,10888,6100};
////volatile float reference[]={0,4208,11470,6100,3873,11487,6100,3883,11493,6100,4343,11091,6100};
//
//// Use this reference
////volatile float reference[]={0,4300,11000,11433,4300,11000,6100,4300,11000,11433,4300,11000,6100};
//
////volatile float reference[]={0,4260,11080,11250,4181,11072,6070,4326,11064,11453,4267,11120,6200};
//
//// References for CyPhySS
//volatile float reference[]={0, 4487, 11056, 11250, 4161, 10949, 6200, 4491, 11363, 11450, 4324, 10949, 6300};
//
//inline void enHigh(){GPIOPinWrite(GPIO_PORTB_BASE,GPIO_PIN_5, GPIO_PIN_5);}
//inline void enLow(){GPIOPinWrite(GPIO_PORTB_BASE,GPIO_PIN_5, 0);}
//
//
////volatile int flag_FR_turn=0;
////volatile int flag_FL_turn=0;
////volatile int flag_BR_turn=0;
////volatile int flag_BL_turn=0;
//
//
//volatile int startwalking_flag=0;
//volatile int begin_flag=0;
//volatile int flag=1;
//volatile int counterrx=-1;
//volatile int timer_count=0;
//volatile int flag_turn=0;
//volatile int flag_turnr=0;
//volatile int flag_turnl=0;
//volatile int flag_stomp=0;
//volatile int loop_delay=0;
//volatile float blxoffset=0;
//volatile float blyoffset=0;
//volatile float brxoffset=0;
//volatile float bryoffset=0;
//volatile float flyoffset=0;
//volatile float fryoffset=0;
//volatile float flag_cal=0;
//volatile float abd_multiplier=0;
//
///*volatile float FLx[]={-0.0939,-0.0831,-0.0702,-0.0539,-0.0409,-0.0369,-0.0329,-0.0289,-0.0248,-0.0208,-0.0180,-0.0168,-0.0157,-0.0145,-0.0133,-0.0121,-0.0110,-0.0098,-0.0086,-0.0074,-0.0063,-0.0051,-0.0039,-0.0027,-0.0016,-0.0004,0.0008,0.0020,0.0037,0.0055,0.0074,0.0092,0.0111,0.0129,0.0148,0.0168,0.0193,0.0217,0.0241,0.0265,0.0289,0.0305,0.0322,0.0338,0.0354,0.0370,0.0372,0.0370,0.0368,0.0365,0.0363,0.0361,0.0359,0.0357,0.0355,0.0353,0.0352,0.0347,0.0334,0.0321,0.0309,0.0294,0.0269,0.0244,0.0218,0.0195,0.0173,0.0152,0.0130,0.0109,0.0087,0.0066,0.0044,0.0022,0.0001,-0.0021,-0.0042,-0.0064,-0.0085,-0.0107,-0.0128,-0.0155,-0.0181,-0.0208,-0.0234,-0.0261,-0.0287,-0.0314,-0.0363,-0.0418,-0.0473,-0.0528,-0.0583,-0.0641,-0.0719,-0.0802,-0.0912,-0.0949,-0.0949,-0.0939};
//
//volatile float BLx[]={0.0368,0.0365,0.0363,0.0361,0.0359,0.0357,0.0355,0.0353,0.0352,0.0347,0.0334,0.0321,0.0309,0.0294,0.0269,0.0244,0.0218,0.0195,0.0173,0.0152,0.0130,0.0109,0.0087,0.0066,0.0044,0.0022,0.0001,-0.0021,-0.0042,-0.0064,-0.0085,-0.0107,-0.0128,-0.0155,-0.0181,-0.0208,-0.0234,-0.0261,-0.0287,-0.0314,-0.0363,-0.0418,-0.0473,-0.0528,-0.0583,-0.0641,-0.0719,-0.0802,-0.0912,-0.0949,-0.0949,-0.0939,-0.0939,-0.0831,-0.0702,-0.0539,-0.0409,-0.0369,-0.0329,-0.0289,-0.0248,-0.0208,-0.0180,-0.0168,-0.0157,-0.0145,-0.0133,-0.0121,-0.0110,-0.0098,-0.0086,-0.0074,-0.0063,-0.0051,-0.0039,-0.0027,-0.0016,-0.0004,0.0008,0.0020,0.0037,0.0055,0.0074,0.0092,0.0111,0.0129,0.0148,0.0168,0.0193,0.0217,0.0241,0.0265,0.0289,0.0305,0.0322,0.0338,0.0354,0.0370,0.0372,0.0370};
//
//volatile float FRx[]={0.0368,0.0365,0.0363,0.0361,0.0359,0.0357,0.0355,0.0353,0.0352,0.0347,0.0334,0.0321,0.0309,0.0294,0.0269,0.0244,0.0218,0.0195,0.0173,0.0152,0.0130,0.0109,0.0087,0.0066,0.0044,0.0022,0.0001,-0.0021,-0.0042,-0.0064,-0.0085,-0.0107,-0.0128,-0.0155,-0.0181,-0.0208,-0.0234,-0.0261,-0.0287,-0.0314,-0.0363,-0.0418,-0.0473,-0.0528,-0.0583,-0.0641,-0.0719,-0.0802,-0.0912,-0.0949,-0.0949,-0.0939,-0.0939,-0.0831,-0.0702,-0.0539,-0.0409,-0.0369,-0.0329,-0.0289,-0.0248,-0.0208,-0.0180,-0.0168,-0.0157,-0.0145,-0.0133,-0.0121,-0.0110,-0.0098,-0.0086,-0.0074,-0.0063,-0.0051,-0.0039,-0.0027,-0.0016,-0.0004,0.0008,0.0020,0.0037,0.0055,0.0074,0.0092,0.0111,0.0129,0.0148,0.0168,0.0193,0.0217,0.0241,0.0265,0.0289,0.0305,0.0322,0.0338,0.0354,0.0370,0.0372,0.0370};
//
//volatile float BRx[]={-0.0939,-0.0831,-0.0702,-0.0539,-0.0409,-0.0369,-0.0329,-0.0289,-0.0248,-0.0208,-0.0180,-0.0168,-0.0157,-0.0145,-0.0133,-0.0121,-0.0110,-0.0098,-0.0086,-0.0074,-0.0063,-0.0051,-0.0039,-0.0027,-0.0016,-0.0004,0.0008,0.0020,0.0037,0.0055,0.0074,0.0092,0.0111,0.0129,0.0148,0.0168,0.0193,0.0217,0.0241,0.0265,0.0289,0.0305,0.0322,0.0338,0.0354,0.0370,0.0372,0.0370,0.0368,0.0365,0.0363,0.0361,0.0359,0.0357,0.0355,0.0353,0.0352,0.0347,0.0334,0.0321,0.0309,0.0294,0.0269,0.0244,0.0218,0.0195,0.0173,0.0152,0.0130,0.0109,0.0087,0.0066,0.0044,0.0022,0.0001,-0.0021,-0.0042,-0.0064,-0.0085,-0.0107,-0.0128,-0.0155,-0.0181,-0.0208,-0.0234,-0.0261,-0.0287,-0.0314,-0.0363,-0.0418,-0.0473,-0.0528,-0.0583,-0.0641,-0.0719,-0.0802,-0.0912,-0.0949,-0.0949,-0.0939};
//
//volatile float FLy[]={-0.1992,-0.1941,-0.1905,-0.1896,-0.1896,-0.1892,-0.1888,-0.1884,-0.1880,-0.1876,-0.1872,-0.1868,-0.1863,-0.1859,-0.1854,-0.1850,-0.1845,-0.1841,-0.1836,-0.1832,-0.1827,-0.1823,-0.1818,-0.1814,-0.1809,-0.1805,-0.1800,-0.1796,-0.1792,-0.1788,-0.1784,-0.1780,-0.1776,-0.1772,-0.1768,-0.1767,-0.1772,-0.1777,-0.1782,-0.1787,-0.1793,-0.1812,-0.1831,-0.1850,-0.1869,-0.1888,-0.1911,-0.1936,-0.1960,-0.1985,-0.2009,-0.2033,-0.2058,-0.2083,-0.2108,-0.2132,-0.2157,-0.2181,-0.2203,-0.2226,-0.2248,-0.2268,-0.2276,-0.2285,-0.2294,-0.2299,-0.2299,-0.2300,-0.2300,-0.2301,-0.2302,-0.2302,-0.2303,-0.2304,-0.2304,-0.2305,-0.2305,-0.2306,-0.2307,-0.2307,-0.2308,-0.2305,-0.2301,-0.2298,-0.2294,-0.2291,-0.2287,-0.2284,-0.2278,-0.2272,-0.2266,-0.2260,-0.2254,-0.2245,-0.2222,-0.2198,-0.2166,-0.2112,-0.2052,-0.1992};
//
//volatile float BLy[]={-0.1960,-0.1985,-0.2009,-0.2033,-0.2058,-0.2083,-0.2108,-0.2132,-0.2157,-0.2181,-0.2203,-0.2226,-0.2248,-0.2268,-0.2276,-0.2285,-0.2294,-0.2299,-0.2299,-0.2300,-0.2300,-0.2301,-0.2302,-0.2302,-0.2303,-0.2304,-0.2304,-0.2305,-0.2305,-0.2306,-0.2307,-0.2307,-0.2308,-0.2305,-0.2301,-0.2298,-0.2294,-0.2291,-0.2287,-0.2284,-0.2278,-0.2272,-0.2266,-0.2260,-0.2254,-0.2245,-0.2222,-0.2198,-0.2166,-0.2112,-0.2052,-0.1992,-0.1992,-0.1941,-0.1905,-0.1896,-0.1896,-0.1892,-0.1888,-0.1884,-0.1880,-0.1876,-0.1872,-0.1868,-0.1863,-0.1859,-0.1854,-0.1850,-0.1845,-0.1841,-0.1836,-0.1832,-0.1827,-0.1823,-0.1818,-0.1814,-0.1809,-0.1805,-0.1800,-0.1796,-0.1792,-0.1788,-0.1784,-0.1780,-0.1776,-0.1772,-0.1768,-0.1767,-0.1772,-0.1777,-0.1782,-0.1787,-0.1793,-0.1812,-0.1831,-0.1850,-0.1869,-0.1888,-0.1911,-0.1936};
//
//volatile float FRy[]={-0.1960,-0.1985,-0.2009,-0.2033,-0.2058,-0.2083,-0.2108,-0.2132,-0.2157,-0.2181,-0.2203,-0.2226,-0.2248,-0.2268,-0.2276,-0.2285,-0.2294,-0.2299,-0.2299,-0.2300,-0.2300,-0.2301,-0.2302,-0.2302,-0.2303,-0.2304,-0.2304,-0.2305,-0.2305,-0.2306,-0.2307,-0.2307,-0.2308,-0.2305,-0.2301,-0.2298,-0.2294,-0.2291,-0.2287,-0.2284,-0.2278,-0.2272,-0.2266,-0.2260,-0.2254,-0.2245,-0.2222,-0.2198,-0.2166,-0.2112,-0.2052,-0.1992,-0.1992,-0.1941,-0.1905,-0.1896,-0.1896,-0.1892,-0.1888,-0.1884,-0.1880,-0.1876,-0.1872,-0.1868,-0.1863,-0.1859,-0.1854,-0.1850,-0.1845,-0.1841,-0.1836,-0.1832,-0.1827,-0.1823,-0.1818,-0.1814,-0.1809,-0.1805,-0.1800,-0.1796,-0.1792,-0.1788,-0.1784,-0.1780,-0.1776,-0.1772,-0.1768,-0.1767,-0.1772,-0.1777,-0.1782,-0.1787,-0.1793,-0.1812,-0.1831,-0.1850,-0.1869,-0.1888,-0.1911,-0.1936};
//
//volatile float BRy[]={-0.1992,-0.1941,-0.1905,-0.1896,-0.1896,-0.1892,-0.1888,-0.1884,-0.1880,-0.1876,-0.1872,-0.1868,-0.1863,-0.1859,-0.1854,-0.1850,-0.1845,-0.1841,-0.1836,-0.1832,-0.1827,-0.1823,-0.1818,-0.1814,-0.1809,-0.1805,-0.1800,-0.1796,-0.1792,-0.1788,-0.1784,-0.1780,-0.1776,-0.1772,-0.1768,-0.1767,-0.1772,-0.1777,-0.1782,-0.1787,-0.1793,-0.1812,-0.1831,-0.1850,-0.1869,-0.1888,-0.1911,-0.1936,-0.1960,-0.1985,-0.2009,-0.2033,-0.2058,-0.2083,-0.2108,-0.2132,-0.2157,-0.2181,-0.2203,-0.2226,-0.2248,-0.2268,-0.2276,-0.2285,-0.2294,-0.2299,-0.2299,-0.2300,-0.2300,-0.2301,-0.2302,-0.2302,-0.2303,-0.2304,-0.2304,-0.2305,-0.2305,-0.2306,-0.2307,-0.2307,-0.2308,-0.2305,-0.2301,-0.2298,-0.2294,-0.2291,-0.2287,-0.2284,-0.2278,-0.2272,-0.2266,-0.2260,-0.2254,-0.2245,-0.2222,-0.2198,-0.2166,-0.2112,-0.2052,-0.1992};
//*/
//volatile float spine13[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//
//volatile float spine14[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//
//
//
//volatile float FLx[]={0.0012,-0.0040,-0.0096,-0.0156,-0.0214,-0.0267,-0.0319,-0.0373,-0.0426,-0.0476,-0.0520,-0.0559,-0.0597,-0.0636,-0.0673,-0.0702,-0.0723,-0.0741,-0.0760,-0.0780,-0.0796,-0.0800,-0.0796,-0.0792,-0.0794,-0.0806,-0.0820,-0.0822,-0.0805,-0.0773,-0.0734,-0.0699,-0.0678,-0.0665,-0.0658,-0.0645,-0.0614,-0.0555,-0.0488,-0.0430,-0.0386,-0.0345,-0.0297,-0.0252,-0.0217,-0.0189,-0.0153,-0.0113,-0.0079,-0.0052,-0.0023,0.0014,0.0050,0.0079,0.0105,0.0137,0.0170,0.0198,0.0222,0.0247,0.0276,0.0303,0.0325,0.0346,0.0370,0.0393,0.0413,0.0431,0.0449,0.0468,0.0483,0.0496,0.0508,0.0521,0.0531,0.0539,0.0545,0.0552,0.0555,0.0552,0.0544,0.0538,0.0541,0.0543,0.0542,0.0539,0.0531,0.0511,0.0477,0.0437,0.0400,0.0370,0.0342,0.0310,0.0274,0.0235,0.0195,0.0152,0.0107,0.0060};
//
//volatile float BLx[]={-0.0008,0.0025,0.0060,0.0090,0.0117,0.0146,0.0178,0.0207,0.0233,0.0258,0.0284,0.0311,0.0334,0.0356,0.0377,0.0399,0.0419,0.0438,0.0454,0.0470,0.0485,0.0498,0.0510,0.0520,0.0528,0.0530,0.0530,0.0526,0.0521,0.0519,0.0519,0.0522,0.0529,0.0529,0.0523,0.0512,0.0499,0.0487,0.0473,0.0454,0.0430,0.0403,0.0373,0.0343,0.0309,0.0272,0.0232,0.0191,0.0148,0.0104,0.0058,0.0011,-0.0038,-0.0089,-0.0140,-0.0191,-0.0242,-0.0291,-0.0339,-0.0385,-0.0429,-0.0472,-0.0512,-0.0551,-0.0586,-0.0619,-0.0650,-0.0678,-0.0704,-0.0727,-0.0747,-0.0764,-0.0777,-0.0785,-0.0790,-0.0788,-0.0774,-0.0751,-0.0725,-0.0700,-0.0677,-0.0651,-0.0599,-0.0565,-0.0535,-0.0509,-0.0480,-0.0440,-0.0401,-0.0369,-0.0343,-0.0311,-0.0270,-0.0231,-0.0200,-0.0174,-0.0140,-0.0101,-0.0066,-0.0037};
//
//volatile float FRx[]={-0.0010,0.0025,0.0060,0.0090,0.0117,0.0147,0.0177,0.0205,0.0231,0.0256,0.0283,0.0308,0.0330,0.0353,0.0375,0.0397,0.0416,0.0434,0.0451,0.0469,0.0485,0.0498,0.0508,0.0518,0.0528,0.0537,0.0545,0.0550,0.0549,0.0543,0.0537,0.0534,0.0533,0.0535,0.0538,0.0540,0.0534,0.0513,0.0478,0.0439,0.0403,0.0373,0.0343,0.0309,0.0271,0.0233,0.0195,0.0154,0.0111,0.0066,0.0018,-0.0034,-0.0089,-0.0145,-0.0200,-0.0257,-0.0313,-0.0367,-0.0416,-0.0463,-0.0509,-0.0555,-0.0596,-0.0630,-0.0662,-0.0692,-0.0720,-0.0743,-0.0761,-0.0774,-0.0785,-0.0793,-0.0797,-0.0797,-0.0794,-0.0794,-0.0793,-0.0789,-0.0776,-0.0755,-0.0726,-0.0694,-0.0661,-0.0643,-0.0633,-0.0622,-0.0596,-0.0536,-0.0467,-0.0409,-0.0369,-0.0333,-0.0286,-0.0241,-0.0207,-0.0178,-0.0142,-0.0102,-0.0067,-0.0039};
//
//volatile float BRx[]={0.0052,0.0005,-0.0045,-0.0095,-0.0146,-0.0196,-0.0245,-0.0294,-0.0340,-0.0385,-0.0428,-0.0470,-0.0510,-0.0546,-0.0581,-0.0615,-0.0646,-0.0674,-0.0698,-0.0720,-0.0740,-0.0757,-0.0771,-0.0780,-0.0783,-0.0783,-0.0776,-0.0755,-0.0727,-0.0696,-0.0667,-0.0641,-0.0617,-0.0582,-0.0547,-0.0514,-0.0483,-0.0451,-0.0414,-0.0378,-0.0345,-0.0313,-0.0278,-0.0240,-0.0206,-0.0176,-0.0145,-0.0108,-0.0072,-0.0041,-0.0013,0.0019,0.0053,0.0084,0.0113,0.0141,0.0171,0.0201,0.0228,0.0254,0.0280,0.0306,0.0330,0.0353,0.0375,0.0397,0.0417,0.0436,0.0453,0.0470,0.0486,0.0499,0.0510,0.0521,0.0529,0.0533,0.0534,0.0531,0.0528,0.0526,0.0527,0.0529,0.0525,0.0523,0.0516,0.0507,0.0494,0.0480,0.0464,0.0446,0.0423,0.0397,0.0367,0.0334,0.0301,0.0266,0.0228,0.0185,0.0141,0.0097};
//
//volatile float FLy[]={-0.2220,-0.2231,-0.2241,-0.2252,-0.2259,-0.2259,-0.2256,-0.2252,-0.2248,-0.2243,-0.2234,-0.2222,-0.2209,-0.2197,-0.2187,-0.2174,-0.2160,-0.2143,-0.2128,-0.2115,-0.2104,-0.2092,-0.2077,-0.2060,-0.2046,-0.2030,-0.2012,-0.1992,-0.1970,-0.1946,-0.1919,-0.1891,-0.1870,-0.1855,-0.1845,-0.1828,-0.1815,-0.1811,-0.1808,-0.1809,-0.1819,-0.1825,-0.1819,-0.1807,-0.1802,-0.1807,-0.1806,-0.1794,-0.1784,-0.1786,-0.1792,-0.1787,-0.1778,-0.1778,-0.1787,-0.1790,-0.1785,-0.1784,-0.1793,-0.1803,-0.1805,-0.1805,-0.1813,-0.1827,-0.1836,-0.1841,-0.1849,-0.1865,-0.1880,-0.1889,-0.1899,-0.1916,-0.1935,-0.1950,-0.1962,-0.1979,-0.2001,-0.2020,-0.2036,-0.2054,-0.2075,-0.2084,-0.2058,-0.2039,-0.2021,-0.2008,-0.2000,-0.1995,-0.2002,-0.2020,-0.2042,-0.2061,-0.2074,-0.2087,-0.2105,-0.2129,-0.2153,-0.2173,-0.2191,-0.2206};
//
//volatile float BLy[]={-0.1774,-0.1771,-0.1763,-0.1759,-0.1764,-0.1769,-0.1768,-0.1766,-0.1770,-0.1780,-0.1787,-0.1790,-0.1794,-0.1805,-0.1817,-0.1827,-0.1836,-0.1847,-0.1861,-0.1876,-0.1890,-0.1905,-0.1920,-0.1937,-0.1954,-0.1967,-0.1975,-0.1977,-0.1977,-0.1976,-0.1973,-0.1967,-0.1959,-0.1954,-0.1954,-0.1960,-0.1972,-0.1985,-0.1996,-0.2006,-0.2018,-0.2035,-0.2053,-0.2070,-0.2084,-0.2098,-0.2114,-0.2129,-0.2142,-0.2152,-0.2162,-0.2170,-0.2175,-0.2179,-0.2182,-0.2183,-0.2182,-0.2179,-0.2174,-0.2168,-0.2161,-0.2152,-0.2142,-0.2130,-0.2119,-0.2107,-0.2093,-0.2079,-0.2065,-0.2051,-0.2037,-0.2023,-0.2008,-0.1993,-0.1978,-0.1965,-0.1958,-0.1952,-0.1944,-0.1937,-0.1932,-0.1927,-0.1925,-0.1910,-0.1897,-0.1890,-0.1883,-0.1872,-0.1855,-0.1841,-0.1837,-0.1833,-0.1822,-0.1806,-0.1797,-0.1798,-0.1795,-0.1784,-0.1773,-0.1771};
//
//volatile float FRy[]={-0.1784,-0.1780,-0.1771,-0.1769,-0.1774,-0.1777,-0.1774,-0.1772,-0.1778,-0.1786,-0.1790,-0.1792,-0.1799,-0.1810,-0.1820,-0.1827,-0.1837,-0.1851,-0.1865,-0.1877,-0.1889,-0.1903,-0.1921,-0.1940,-0.1955,-0.1970,-0.1988,-0.2006,-0.2026,-0.2048,-0.2065,-0.2069,-0.2073,-0.2061,-0.2047,-0.2033,-0.2019,-0.2012,-0.2017,-0.2033,-0.2050,-0.2064,-0.2073,-0.2084,-0.2100,-0.2122,-0.2142,-0.2161,-0.2178,-0.2196,-0.2211,-0.2223,-0.2234,-0.2243,-0.2248,-0.2250,-0.2250,-0.2248,-0.2244,-0.2236,-0.2226,-0.2217,-0.2208,-0.2196,-0.2182,-0.2167,-0.2154,-0.2141,-0.2127,-0.2111,-0.2096,-0.2082,-0.2068,-0.2055,-0.2038,-0.2017,-0.1995,-0.1974,-0.1953,-0.1932,-0.1911,-0.1888,-0.1866,-0.1848,-0.1836,-0.1824,-0.1813,-0.1815,-0.1813,-0.1811,-0.1819,-0.1828,-0.1822,-0.1807,-0.1800,-0.1804,-0.1803,-0.1791,-0.1780,-0.1780};
//
//volatile float BRy[]={-0.2160,-0.2167,-0.2173,-0.2177,-0.2179,-0.2180,-0.2180,-0.2177,-0.2172,-0.2166,-0.2159,-0.2150,-0.2141,-0.2130,-0.2119,-0.2106,-0.2092,-0.2079,-0.2066,-0.2052,-0.2037,-0.2022,-0.2008,-0.1994,-0.1980,-0.1966,-0.1955,-0.1950,-0.1945,-0.1938,-0.1931,-0.1926,-0.1914,-0.1903,-0.1889,-0.1878,-0.1870,-0.1863,-0.1850,-0.1835,-0.1825,-0.1819,-0.1813,-0.1800,-0.1788,-0.1783,-0.1782,-0.1776,-0.1765,-0.1759,-0.1761,-0.1763,-0.1758,-0.1753,-0.1755,-0.1761,-0.1764,-0.1763,-0.1765,-0.1774,-0.1782,-0.1787,-0.1792,-0.1802,-0.1814,-0.1824,-0.1834,-0.1846,-0.1860,-0.1875,-0.1888,-0.1902,-0.1919,-0.1937,-0.1953,-0.1962,-0.1964,-0.1961,-0.1958,-0.1954,-0.1950,-0.1945,-0.1945,-0.1943,-0.1946,-0.1953,-0.1964,-0.1977,-0.1991,-0.2003,-0.2017,-0.2032,-0.2050,-0.2068,-0.2084,-0.2099,-0.2113,-0.2126,-0.2139,-0.2151};
//
//
////volatile float spine13[]={0.0052,0.0065,0.0080,0.0095,0.0109,0.0121,0.0129,0.0131,0.0126,0.0117,0.0107,0.0098,0.0090,0.0083,0.0074,0.0064,0.0053,0.0042,0.0031,0.0016,-0.0003,-0.0024,-0.0043,-0.0055,-0.0065,-0.0078,-0.0100,-0.0131,-0.0164,-0.0192,-0.0213,-0.0227,-0.0238,-0.0248,-0.0258,-0.0262,-0.0262,-0.0259,-0.0258,-0.0261,-0.0269,-0.0280,-0.0293,-0.0301,-0.0301,-0.0292,-0.0277,-0.0258,-0.0238,-0.0222,-0.0212,-0.0207,-0.0205,-0.0201,-0.0193,-0.0182,-0.0170,-0.0159,-0.0149,-0.0140,-0.0129,-0.0115,-0.0097,-0.0074,-0.0048,-0.0022,0.0004,0.0025,0.0043,0.0059,0.0074,0.0090,0.0107,0.0124,0.0138,0.0147,0.0149,0.0145,0.0141,0.0144,0.0154,0.0164,0.0167,0.0158,0.0145,0.0135,0.0134,0.0140,0.0145,0.0144,0.0136,0.0129,0.0127,0.0134,0.0145,0.0158,0.0168,0.0173,0.0168,0.0150};
//
////volatile float spine14[]={-0.0191,-0.0204,-0.0212,-0.0215,-0.0215,-0.0213,-0.0210,-0.0208,-0.0206,-0.0203,-0.0196,-0.0184,-0.0170,-0.0154,-0.0138,-0.0122,-0.0103,-0.0082,-0.0061,-0.0044,-0.0034,-0.0033,-0.0037,-0.0039,-0.0040,-0.0038,-0.0037,-0.0035,-0.0030,-0.0022,-0.0011,0.0001,0.0010,0.0019,0.0031,0.0049,0.0069,0.0085,0.0091,0.0085,0.0072,0.0057,0.0046,0.0041,0.0044,0.0053,0.0063,0.0073,0.0083,0.0094,0.0102,0.0101,0.0089,0.0069,0.0048,0.0029,0.0016,0.0008,0.0002,-0.0004,-0.0011,-0.0017,-0.0022,-0.0024,-0.0021,-0.0016,-0.0010,-0.0012,-0.0023,-0.0046,-0.0074,-0.0098,-0.0113,-0.0117,-0.0118,-0.0121,-0.0131,-0.0144,-0.0156,-0.0158,-0.0152,-0.0141,-0.0129,-0.0122,-0.0118,-0.0118,-0.0123,-0.0131,-0.0141,-0.0149,-0.0154,-0.0158,-0.0162,-0.0170,-0.0181,-0.0194,-0.0208,-0.0222,-0.0234,-0.0243};
//
//
///*const int motor5[]={9979,9979,9979,9980,9980,9981,9982,9983,9984,9985,9987,9989,9991,9993,9995,9998,10001,10004,10008,10012,10016,10021,10026,10031,10036,10042,10049,10055,10062,10069,10076,10084,10092,10100,10108,10117,10126,10135,10144,10153,10163,10173,10183,10193,10203,10213,10223,10233,10244,10254,10264,10275,10285,10295,10305,10315,10325,10335,10345,10354,10364,10373,10382,10390,10399,10407,10416,10424,10431,10438,10445,10452,10458,10464,10470,10476,10481,10486,10490,10494,10498,10502,10505,10508,10510,10513,10515,10517,10518,10520,10521,10522,10523,10524,10524,10525,10525,10525,10525,10525};
//
//const int motor4[]={5590,5588,5586,5583,5578,5572,5565,5558,5550,5541,5531,5520,5509,5496,5483,5470,5455,5440,5425,5408,5391,5373,5355,5337,5319,5299,5280,5260,5240,5220,5198,5178,5156,5135,5113,5091,5070,5047,5026,5004,4981,4959,4936,4914,4891,4869,4847,4825,4802,4780,4758,4736,4714,4693,4671,4649,4628,4606,4586,4564,4543,4523,4502,4483,4463,4443,4424,4404,4386,4367,4348,4331,4313,4295,4278,4261,4245,4229,4214,4198,4183,4168,4155,4141,4129,4117,4105,4094,4083,4074,4065,4057,4050,4044,4038,4033,4030,4027,4026,4026};
//
//const int motor1[]={5656,5656,5656,5655,5655,5654,5653,5652,5651,5650,5648,5646,5644,5642,5640,5637,5634,5631,5627,5623,5619,5614,5609,5604,5599,5593,5586,5580,5573,5566,5559,5551,5543,5535,5527,5518,5509,5500,5491,5482,5472,5462,5452,5442,5432,5422,5412,5402,5391,5381,5371,5360,5350,5340,5330,5320,5310,5300,5290,5281,5271,5262,5253,5245,5236,5228,5219,5211,5204,5197,5190,5183,5177,5171,5165,5159,5154,5149,5145,5141,5137,5133,5130,5127,5125,5122,5120,5118,5117,5115,5114,5113,5112,5111,5111,5110,5110,5110,5110,5110};
//
//const int motor2[]={9828,9830,9832,9835,9840,9846,9853,9860,9868,9877,9887,9898,9909,9922,9935,9948,9963,9978,9993,10010,10027,10045,10063,10081,10099,10119,10138,10158,10178,10198,10220,10240,10262,10283,10305,10327,10348,10371,10392,10414,10437,10459,10482,10504,10527,10549,10571,10593,10616,10638,10660,10682,10704,10725,10747,10769,10790,10812,10832,10854,10875,10895,10916,10935,10955,10975,10994,11014,11032,11051,11070,11087,11105,11123,11140,11157,11173,11189,11204,11220,11235,11250,11263,11277,11289,11301,11313,11324,11335,11344,11353,11361,11368,11374,11380,11385,11388,11391,11392,11392};
//
//const int motor11[]={9599,9599,9599,9600,9600,9601,9602,9603,9604,9605,9607,9609,9611,9613,9615,9618,9621,9624,9628,9632,9636,9641,9646,9651,9656,9662,9669,9675,9682,9689,9696,9704,9712,9720,9728,9737,9746,9755,9764,9773,9783,9793,9803,9813,9823,9833,9843,9853,9864,9874,9884,9895,9905,9915,9925,9935,9945,9955,9965,9974,9984,9993,10002,10010,10019,10027,10036,10044,10051,10058,10065,10072,10078,10084,10090,10096,10101,10106,10110,10114,10118,10122,10125,10128,10130,10133,10135,10137,10138,10140,10141,10142,10143,10144,10144,10145,10145,10145,10145,10145};
//
//const int motor10[]={5589,5587,5585,5582,5577,5571,5564,5557,5549,5540,5530,5519,5508,5495,5482,5469,5454,5439,5424,5407,5390,5372,5354,5336,5318,5298,5279,5259,5239,5219,5197,5177,5155,5134,5112,5090,5069,5046,5025,5003,4980,4958,4935,4913,4890,4868,4846,4824,4801,4779,4757,4735,4713,4692,4670,4648,4627,4605,4585,4563,4542,4522,4501,4482,4462,4442,4423,4403,4385,4366,4347,4330,4312,4294,4277,4260,4244,4228,4213,4197,4182,4167,4154,4140,4128,4116,4104,4093,4082,4073,4064,4056,4049,4043,4037,4032,4029,4026,4025,4025};
//
//const int motor7[]={5318,5318,5318,5317,5317,5316,5315,5314,5313,5312,5310,5308,5306,5304,5302,5299,5296,5293,5289,5285,5281,5276,5271,5266,5261,5255,5248,5242,5235,5228,5221,5213,5205,5197,5189,5180,5171,5162,5153,5144,5134,5124,5114,5104,5094,5084,5074,5064,5053,5043,5033,5022,5012,5002,4992,4982,4972,4962,4952,4943,4933,4924,4915,4907,4898,4890,4881,4873,4866,4859,4852,4845,4839,4833,4827,4821,4816,4811,4807,4803,4799,4795,4792,4789,4787,4784,4782,4780,4779,4777,4776,4775,4774,4773,4773,4772,4772,4772,4772,4772};
//
//const int motor8[]={9851,9853,9855,9858,9863,9869,9876,9883,9891,9900,9910,9921,9932,9945,9958,9971,9986,10001,10016,10033,10050,10068,10086,10104,10122,10142,10161,10181,10201,10221,10243,10263,10285,10306,10328,10350,10371,10394,10415,10437,10460,10482,10505,10527,10550,10572,10594,10616,10639,10661,10683,10705,10727,10748,10770,10792,10813,10835,10855,10877,10898,10918,10939,10958,10978,10998,11017,11037,11055,11074,11093,11110,11128,11146,11163,11180,11196,11212,11227,11243,11258,11273,11286,11300,11312,11324,11336,11347,11358,11367,11376,11384,11391,11397,11403,11408,11411,11414,11415,11415};
//*/
//
//
///*const float FLyt[]={-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200};
//
//const float BLyt[]={-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200};
//
//const float FRyt[]={-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200};
//
//const float BRyt[]={-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200};
//
//const float abduction6t[]={0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000};
//
//const float abduction12t[]={0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490};
//
//const float abduction3t[]={0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490};
//
//const float abduction9t[]={0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000};
//*/
//
//
//const float FLyt[]={-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200};
//
//const float BLyt[]={-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660};
//
//const float FRyt[]={-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660};
//
//const float BRyt[]={-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2110,-0.2030,-0.1940,-0.1860,-0.1790,-0.1720,-0.1660,-0.1610,-0.1570,-0.1530,-0.1510,-0.1500,-0.1500,-0.1510,-0.1530,-0.1570,-0.1610,-0.1660,-0.1720,-0.1790,-0.1860,-0.1940,-0.2030,-0.2110,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200,-0.2200};
//
//const float abduction6t[]={0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350};
//
//const float abduction12t[]={0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870};
//
//const float abduction3t[]={0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870};
//
//const float abduction9t[]={0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350,0.2530,0.2700,0.2880,0.3040,0.3200,0.3350,0.3490,0.3610,0.3710,0.3780,0.3830,0.3840,0.3820,0.3760,0.3650,0.3490,0.3290,0.3040,0.2760,0.2450,0.2120,0.1780,0.1460,0.1150,0.0870,0.0630,0.0430,0.0270,0.0140,0.0060,0.0010,0.0000,0.0010,0.0050,0.0110,0.0190,0.0290,0.0400,0.0530,0.0660,0.0810,0.0960,0.1120,0.1290,0.1460,0.1630,0.1810,0.1990,0.2170,0.2350};
//
//
//
///*
// volatile float FLx[]={-0.0154,-0.0194,-0.0236,-0.0280,-0.0320,-0.0353,-0.0386,-0.0424,-0.0460,-0.0489,-0.0518,-0.0547,-0.0574,-0.0600,-0.0626,-0.0650,-0.0670,-0.0688,-0.0707,-0.0724,-0.0738,-0.0750,-0.0761,-0.0770,-0.0776,-0.0779,-0.0779,-0.0777,-0.0773,-0.0767,-0.0757,-0.0746,-0.0732,-0.0716,-0.0698,-0.0678,-0.0656,-0.0631,-0.0605,-0.0576,-0.0546,-0.0513,-0.0478,-0.0441,-0.0402,-0.0360,-0.0316,-0.0270,-0.0222,-0.0172,-0.0120,-0.0065,-0.0009,0.0048,0.0104,0.0159,0.0213,0.0265,0.0319,0.0373,0.0419,0.0464,0.0510,0.0549,0.0584,0.0617,0.0645,0.0668,0.0688,0.0704,0.0716,0.0724,0.0728,0.0729,0.0725,0.0719,0.0709,0.0696,0.0680,0.0662,0.0640,0.0615,0.0588,0.0559,0.0527,0.0492,0.0455,0.0417,0.0376,0.0333,0.0288,0.0242,0.0194,0.0146,0.0105,0.0067,0.0026,-0.0020,-0.0067,-0.0112};
//
// volatile float BLx[]={-0.0060,-0.0013,0.0032,0.0088,0.0149,0.0204,0.0256,0.0309,0.0360,0.0401,0.0451,0.0497,0.0522,0.0552,0.0592,0.0621,0.0634,0.0652,0.0674,0.0683,0.0684,0.0689,0.0690,0.0686,0.0679,0.0670,0.0659,0.0646,0.0631,0.0613,0.0593,0.0572,0.0549,0.0524,0.0497,0.0469,0.0438,0.0406,0.0373,0.0338,0.0302,0.0264,0.0225,0.0185,0.0144,0.0102,0.0060,0.0017,-0.0027,-0.0070,-0.0114,-0.0158,-0.0201,-0.0243,-0.0284,-0.0323,-0.0360,-0.0396,-0.0429,-0.0458,-0.0483,-0.0505,-0.0526,-0.0544,-0.0559,-0.0571,-0.0580,-0.0588,-0.0594,-0.0599,-0.0602,-0.0605,-0.0606,-0.0607,-0.0606,-0.0605,-0.0603,-0.0599,-0.0595,-0.0590,-0.0585,-0.0577,-0.0569,-0.0559,-0.0548,-0.0535,-0.0521,-0.0505,-0.0486,-0.0466,-0.0443,-0.0418,-0.0391,-0.0361,-0.0328,-0.0290,-0.0250,-0.0210,-0.0169,-0.0117};
//
// volatile float FRx[]={-0.0168,-0.0207,-0.0250,-0.0294,-0.0334,-0.0366,-0.0399,-0.0436,-0.0471,-0.0500,-0.0527,-0.0555,-0.0581,-0.0607,-0.0631,-0.0653,-0.0672,-0.0690,-0.0707,-0.0723,-0.0736,-0.0747,-0.0757,-0.0765,-0.0771,-0.0773,-0.0773,-0.0771,-0.0767,-0.0760,-0.0751,-0.0740,-0.0726,-0.0710,-0.0693,-0.0673,-0.0650,-0.0626,-0.0600,-0.0571,-0.0541,-0.0508,-0.0473,-0.0436,-0.0397,-0.0355,-0.0311,-0.0265,-0.0217,-0.0167,-0.0115,-0.0059,-0.0003,0.0053,0.0109,0.0165,0.0219,0.0272,0.0323,0.0378,0.0427,0.0469,0.0515,0.0554,0.0589,0.0621,0.0648,0.0671,0.0690,0.0705,0.0715,0.0723,0.0726,0.0726,0.0722,0.0715,0.0705,0.0692,0.0676,0.0656,0.0634,0.0610,0.0583,0.0553,0.0521,0.0486,0.0450,0.0411,0.0370,0.0327,0.0283,0.0237,0.0189,0.0141,0.0095,0.0054,0.0011,-0.0035,-0.0081,-0.0126};
//
// volatile float BRx[]={-0.0072,-0.0023,0.0025,0.0084,0.0146,0.0195,0.0237,0.0294,0.0350,0.0395,0.0433,0.0477,0.0523,0.0559,0.0576,0.0599,0.0632,0.0651,0.0658,0.0672,0.0685,0.0686,0.0684,0.0682,0.0676,0.0667,0.0655,0.0642,0.0627,0.0610,0.0591,0.0570,0.0547,0.0522,0.0496,0.0467,0.0437,0.0405,0.0372,0.0337,0.0300,0.0263,0.0224,0.0184,0.0143,0.0102,0.0059,0.0016,-0.0027,-0.0071,-0.0115,-0.0158,-0.0201,-0.0243,-0.0284,-0.0324,-0.0361,-0.0396,-0.0430,-0.0459,-0.0483,-0.0505,-0.0526,-0.0544,-0.0560,-0.0572,-0.0581,-0.0588,-0.0594,-0.0599,-0.0602,-0.0604,-0.0606,-0.0606,-0.0605,-0.0604,-0.0602,-0.0598,-0.0594,-0.0589,-0.0583,-0.0576,-0.0568,-0.0558,-0.0547,-0.0534,-0.0520,-0.0503,-0.0485,-0.0464,-0.0442,-0.0417,-0.0389,-0.0360,-0.0327,-0.0290,-0.0250,-0.0210,-0.0170,-0.0126};
//
// volatile float FLy[]={-0.1862,-0.1845,-0.1827,-0.1812,-0.1801,-0.1790,-0.1775,-0.1761,-0.1753,-0.1745,-0.1733,-0.1722,-0.1715,-0.1707,-0.1698,-0.1691,-0.1685,-0.1678,-0.1672,-0.1667,-0.1663,-0.1658,-0.1654,-0.1651,-0.1649,-0.1647,-0.1647,-0.1647,-0.1649,-0.1653,-0.1658,-0.1665,-0.1673,-0.1682,-0.1692,-0.1704,-0.1716,-0.1730,-0.1743,-0.1758,-0.1773,-0.1788,-0.1803,-0.1817,-0.1832,-0.1846,-0.1859,-0.1871,-0.1882,-0.1892,-0.1900,-0.1905,-0.1908,-0.1909,-0.1908,-0.1905,-0.1899,-0.1893,-0.1888,-0.1880,-0.1870,-0.1866,-0.1858,-0.1847,-0.1842,-0.1837,-0.1829,-0.1824,-0.1820,-0.1816,-0.1814,-0.1813,-0.1813,-0.1814,-0.1817,-0.1821,-0.1825,-0.1831,-0.1838,-0.1846,-0.1854,-0.1864,-0.1874,-0.1884,-0.1894,-0.1905,-0.1916,-0.1926,-0.1936,-0.1946,-0.1955,-0.1963,-0.1971,-0.1976,-0.1972,-0.1954,-0.1932,-0.1913,-0.1896,-0.1879};
//
// volatile float BLy[]={-0.1884,-0.1887,-0.1892,-0.1894,-0.1890,-0.1889,-0.1887,-0.1879,-0.1872,-0.1862,-0.1857,-0.1843,-0.1828,-0.1821,-0.1809,-0.1795,-0.1787,-0.1779,-0.1767,-0.1759,-0.1753,-0.1743,-0.1736,-0.1732,-0.1728,-0.1724,-0.1720,-0.1718,-0.1718,-0.1720,-0.1723,-0.1727,-0.1731,-0.1736,-0.1742,-0.1749,-0.1755,-0.1762,-0.1769,-0.1776,-0.1783,-0.1789,-0.1794,-0.1800,-0.1804,-0.1808,-0.1811,-0.1813,-0.1814,-0.1814,-0.1813,-0.1810,-0.1807,-0.1802,-0.1796,-0.1789,-0.1781,-0.1773,-0.1765,-0.1757,-0.1750,-0.1740,-0.1731,-0.1724,-0.1717,-0.1709,-0.1700,-0.1691,-0.1681,-0.1672,-0.1664,-0.1656,-0.1649,-0.1643,-0.1638,-0.1635,-0.1632,-0.1631,-0.1632,-0.1634,-0.1638,-0.1643,-0.1650,-0.1658,-0.1667,-0.1678,-0.1690,-0.1703,-0.1717,-0.1731,-0.1747,-0.1763,-0.1779,-0.1795,-0.1811,-0.1826,-0.1840,-0.1852,-0.1863,-0.1877};
//
// volatile float FRy[]={-0.1865,-0.1847,-0.1829,-0.1813,-0.1801,-0.1788,-0.1772,-0.1758,-0.1749,-0.1740,-0.1729,-0.1719,-0.1711,-0.1702,-0.1694,-0.1688,-0.1682,-0.1675,-0.1669,-0.1665,-0.1661,-0.1657,-0.1653,-0.1651,-0.1650,-0.1650,-0.1650,-0.1652,-0.1655,-0.1659,-0.1665,-0.1672,-0.1680,-0.1690,-0.1700,-0.1712,-0.1724,-0.1737,-0.1750,-0.1764,-0.1779,-0.1793,-0.1808,-0.1822,-0.1836,-0.1849,-0.1861,-0.1873,-0.1884,-0.1893,-0.1900,-0.1905,-0.1908,-0.1908,-0.1907,-0.1904,-0.1898,-0.1892,-0.1885,-0.1880,-0.1869,-0.1863,-0.1857,-0.1846,-0.1841,-0.1835,-0.1827,-0.1822,-0.1818,-0.1814,-0.1812,-0.1810,-0.1809,-0.1811,-0.1813,-0.1816,-0.1821,-0.1826,-0.1833,-0.1840,-0.1848,-0.1857,-0.1866,-0.1876,-0.1886,-0.1896,-0.1907,-0.1917,-0.1926,-0.1935,-0.1944,-0.1952,-0.1959,-0.1964,-0.1964,-0.1952,-0.1934,-0.1915,-0.1899,-0.1881};
//
// volatile float BRy[]={-0.1881,-0.1881,-0.1885,-0.1891,-0.1887,-0.1882,-0.1880,-0.1875,-0.1862,-0.1857,-0.1846,-0.1838,-0.1827,-0.1814,-0.1801,-0.1792,-0.1779,-0.1768,-0.1762,-0.1753,-0.1744,-0.1738,-0.1734,-0.1725,-0.1715,-0.1710,-0.1710,-0.1712,-0.1714,-0.1717,-0.1720,-0.1725,-0.1730,-0.1736,-0.1743,-0.1750,-0.1757,-0.1764,-0.1770,-0.1777,-0.1784,-0.1790,-0.1795,-0.1800,-0.1804,-0.1808,-0.1811,-0.1812,-0.1813,-0.1813,-0.1812,-0.1809,-0.1806,-0.1801,-0.1795,-0.1789,-0.1781,-0.1773,-0.1765,-0.1758,-0.1750,-0.1740,-0.1731,-0.1725,-0.1718,-0.1709,-0.1700,-0.1691,-0.1682,-0.1673,-0.1665,-0.1657,-0.1650,-0.1644,-0.1639,-0.1635,-0.1633,-0.1632,-0.1633,-0.1635,-0.1639,-0.1644,-0.1651,-0.1659,-0.1668,-0.1679,-0.1691,-0.1704,-0.1718,-0.1732,-0.1748,-0.1763,-0.1780,-0.1796,-0.1812,-0.1827,-0.1840,-0.1852,-0.1862,-0.1872};
// */
//
//
//
///*
// float q_last5[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired5[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired5[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last4[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired4[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired4[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last1[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired1[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired1[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last2[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired2[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired2[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last7[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired7[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired7[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last8[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired8[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired8[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last10[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired10[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired10[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_last11[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float q_desired11[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// float qd_desired11[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// */
//float action1[10] = {0.10764433, -0.22723689,  0.37480827,  0.01160054, -0.18521147,  0.01536253,
//  0.07677522, -0.65195124,  0.0739685,  -0.23965774};
//float theta =0;
//void bodyOrientation(float degp,float degr)
//{
//
//  float l = 0.13505;
//  float b = 0.119615;
//  float FL[]={l,0,b};
//  float BL[]={-l,0,b};
//  float FR[]={l,0,-b};
//  float BR[]={-l,0,-b};
//  float T[3][3];
//  float FT0[]={0,0,0};
//  float FT1[]={0,0,0};
//  float FT2[]={0,0,0};
//  float FT3[]={0,0,0};
//  int i,j,k;
//  float Tr[3][3]={
//      {1,0,0},
//      {0,cosf(degr),sinf(degr)},
//      {0,-sinf(degr),cosf(degr)}
//  };
//  float Tp[3][3]={
//      {cosf(degp),sinf(degp),0},
//      {-sinf(degp),cosf(degp),0},
//      {0,0,1}
//  };
//  for(i=0; i<3; ++i)
//      for(j=0; j<3; ++j)
//      {
//          T[i][j] = 0;
//      }
//  for(i=0; i<3; ++i)
//      for(j=0; j<3; ++j)
//          for(k=0; k<3; ++k)
//          {
//              T[i][j]+=Tr[i][k]*Tp[k][j];
//          }
//
//  for(i=0; i<3; ++i)
//      for(k=0; k<3; ++k)
//      {
//          FT0[i]+=T[i][k]*FL[k];
//      }
//
//  for(i=0; i<3; ++i)
//      for(k=0; k<3; ++k)
//      {
//          FT1[i]+=T[i][k]*BL[k];
//      }
//
//  for(i=0; i<3; ++i)
//      for(k=0; k<3; ++k)
//      {
//          FT2[i]+=T[i][k]*FR[k];
//      }
//
//  for(i=0; i<3; ++i)
//      for(k=0; k<3; ++k)
//      {
//          FT3[i]+=T[i][k]*BR[k];
//      }
//
//  flyoffset=FT0[1];
//  blyoffset=FT1[1];
//  fryoffset=FT2[1];
//  bryoffset=FT3[1];
//
//}
//
//
//  void
//InitConsole_B(void)
//{
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
//  GPIOPinConfigure(GPIO_PB0_U1RX);
//  GPIOPinConfigure(GPIO_PB1_U1TX);
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
//  UARTClockSourceSet(UART1_BASE, UART_CLOCK_PIOSC);
//  GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//  UARTStdioConfig(1, 1250000, 16000000);
//  //UARTStdioConfig(1, 115200, 16000000);
//}
//
//  void
//ConfigureUART(void)
//{
//  //
//  // Enable the GPIO Peripheral used by the UART.
//  //
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
//  GPIOPinConfigure(GPIO_PA0_U0RX);
//  GPIOPinConfigure(GPIO_PA1_U0TX);
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
//  UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
//  GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//  UARTStdioConfig(0, 921600, 16000000);
//  //UARTStdioConfig(0, 115200, 16000000);
//}
//float limiter(float X)
//{
//  //function returns -1 if x < 0, 0 if x==0, 1 if x > 0.
//  float temp=X;
//  if(temp<0)
//      temp=temp*-1;
//  if(temp>1){
//      if(X<0)
//          return -1;
//      else if(X==0)
//          return 0;
//      else if(X==0)
//          return 1;
//  }
//  else
//      return X;
//}
//
//inline float to_degrees(float radians) {
//  return radians * (180.0 / 3.14);
//}
//
//
//int setPos(uint8_t id, unsigned int pos)
//{
//  char txCmd[3];
//  char rxCmd[3];
//  unsigned int rePos;
//  bool flg;
//
//  txCmd[0] = 0x80 + id;               // CMD
//  txCmd[1] = ((pos >> 7) & 0x007F);   // POS_H
//  txCmd[2] = (pos & 0x007F);          // POS_L
//
//  flg = synchronize(txCmd, 3, rxCmd, 3, id);
//  if (flg == false)
//  {
//      return 0;
//  }
//
//  rePos = ((rxCmd[1] << 7) & 0x3F80) + (rxCmd[2] & 0x007F);
//  motor_counter=rePos+motor_counter;
//  return rePos;
//}
//
//
//bool synchronize(char *txBuf, char txLen, char *rxBuf, char rxLen, uint8_t id)
//{   //TimerDisable(TIMER5_BASE, TIMER_A);
//  //TimerIntDisable(TIMER5_BASE);
//  char rxSize;
//  uint32_t ui32Count = 0;
//  int8_t cChar;
//  int numc;
//  //UARTFlushTx(true);
//  //UARTFlushRx();
//  enHigh();
//  //UARTwrite(txBuf,txLen);
//
//  for(numc=0;numc<txLen;numc++)
//  {UARTCharPut(UART1_BASE,*(txBuf+numc));}
//  //UARTCharPut(UART1_BASE,*(txBuf+1));
//  //UARTCharPut(UART1_BASE,*(txBuf+2));
//  while(UARTBusy(UART1_BASE));
//  //SysCtlDelay(400);
//  enLow();
//
//  for(numc=0;numc<rxLen;numc++)
//  {*(rxBuf+numc)=UARTCharGetICS2(UART1_BASE, id);}
//  //*(rxBuf+1)=UARTCharGet(UART1_BASE);
//  //*(rxBuf+3)=UARTCharGet(UART1_BASE);
//  //TimerEnable(TIMER5_BASE, TIMER_A);
//  return true;
//
//
//}
//
//int setTmp(char id, unsigned int tmplim)
//{
//  char txCmd[3];
//  char rxCmd[3];
//  unsigned int reData;
//  bool flg;
//  txCmd[0] = 0xC0 + id;
//  txCmd[1] = 0x04;
//  txCmd[2] = tmplim;
//  flg = synchronize(txCmd, sizeof txCmd, rxCmd, sizeof rxCmd, id);
//  reData = rxCmd[2];
//  return reData;
//}
//
//
//int getCur(uint8_t id)
//{
//  //UARTprintf("getCur\n");
//  char txCmd[2];
//  char rxCmd[3];
//  unsigned int reData;
//  bool flg;
//  txCmd[0] = 0xA0 + id;
//  txCmd[1] = 0x03;
//  flg = synchronize(txCmd, 2, rxCmd, 3, id);
//  reData = rxCmd[2];
//  return reData;
//}
//
//
//int getSpd(uint8_t id)
//{
//  char txCmd[2];
//  char rxCmd[3];
//  unsigned int reData;
//  bool flg;
//  txCmd[0] = 0xA0 + id;    // CMD
//  txCmd[1] = 0x02;         // SC スピード
//  flg = synchronize(txCmd, 2, rxCmd, 3, id);
//  reData = rxCmd[2];
//  return reData;
//}
//
//
//int getTmp(char id)
//{
//  char txCmd[2];
//  char rxCmd[3];
//  unsigned int reData;
//  bool flg;
//  txCmd[0] = 0xA0 + id;    // CMD
//  txCmd[1] = 0x04;
//  flg = synchronize(txCmd, sizeof txCmd, rxCmd, sizeof rxCmd, id);
//  reData = rxCmd[2];
//  return reData;
//}
//
//int getPos(char id) {
//  char txCmd[2];
//  char rxCmd[4];
//  unsigned int reData;
//  bool flg;
//
//  txCmd[0] = 0xA0 + id;    // CMD
//  txCmd[1] = 0x05;
//
//  flg = synchronize(txCmd, sizeof txCmd, rxCmd, sizeof rxCmd, id);
//  reData = ((rxCmd[2] << 7) & 0x3F80) + (rxCmd[3] & 0x007F);
//  return reData;
//}
//void inverse_stoch(float x,float y)
//{
//  //float Leg[] = {0.12,0.15015,0.04,0.15501,0.11187,0.04,0.2532,2.803};
//  //UARTprintf("IK start\n");
//  float xb[] = {0,0};
//  float yb[] = {0,0};
//  float phid[] = {0,0};
//  float psi[] = {0,0};
//  float theta[] = {0,0};
//  float R_base[]= {0,0.035,0,0};
//  float zeta,xm,ym,xi,yi;
//  float R[] = {0,0};
//
//  //float l1 =    *Leg;
//  float l1 =   0.12;
//  float l2 =   0.15015;
//  float l4 =   0.04;
//  float l5 =   0.15501;
//  float le =   0.04;
//  float tq1 =  0.2532;
//  float tq2 =  2.803;
//  float delta = 0.11187;
//
//  float l3,l6;
//
//  xb[0] = R_base[0];
//  xb[1] = R_base[1];
//  yb[0] = R_base[2];
//  yb[1] = R_base[3];
//
//
//  l3 = ((x-xb[0])*(x-xb[0])+(y-yb[0])*(y-yb[0]));
//  l3=sqrtf(l3);
//  //l3 = sqrt(pow((x-xb[0]),2)+pow((y-yb[0]),2));
//  theta[0] = atan2f((y-yb[0]),(x-xb[0]));
//  zeta = (powf(l3,2) - powf(l1,2) -powf(l2,2))/(2*l1*l2);
//  zeta = limiter(zeta);
//  phid[0] = acosf(zeta);
//  psi[0] = atan2f(l2*sinf(phid[0]),(l1+l2*cosf(phid[0])));
//  q1 = theta[0] - psi[0];
//  q2 = q1 + phid[0];
//
//
//  xi = xb[0] + l1*cosf(q1) + 0.04*cosf(q2-tq1);
//  yi = yb[0] + l1*sinf(q1) + 0.04*sinf(q2-tq1);
//  //printf("%f,%f\n",xi,yi);
//  //R[0] = xi;
//  //R[1] = yi;
//  l6=(xi-xb[1])*(xi-xb[1])+(yi-yb[1])*(yi-yb[1]);
//  l6=sqrtf(l6);
//  //l6 = sqrt((pow((xi-xb[1]),2)+pow((yi-yb[1]),2)));
//  theta[1] = atan2f((yi-yb[1]),(xi-xb[1]));
//  zeta=(powf(l6,2) - powf(l4,2) -powf(l5,2))/(2*l5*l4);
//  //printf("%f\n",zeta);
//  zeta = limiter(zeta);
//  phid[1] = acosf(zeta);
//  //printf("%f\n",zeta);
//  psi[1] = atan2f(l5*sinf(phid[1]),(l4+l5*cosf(phid[1])));
//  q3 = theta[1] + psi[1];
//  q4 = q3 - phid[1];
//  //printf("%f,%f\n",phid[1],psi[1]);
//  xm = l4*cosf(q3)+l5*cosf(q4)+xb[1];
//  ym = l4*sinf(q3)+l5*sinf(q4)+yb[1];
//  if (zeta == 1)
//  {
//      l1 = 0.12;
//      l2 = 0.03828;//Leg[1]-Leg[4];
//      l4 = 0.04;
//      l5 = 0.15501;
//      delta = 0.11187;
//      R_base[0]= 1;
//      R_base[1]=-1;
//      R_base[2]=0;
//      R_base[3]=0;
//      xb[0] = R_base[0];
//      xb[1] = R_base[1];
//      yb[0] = R_base[2];
//      yb[1] = R_base[3];
//      l3 = sqrtf(powf((xm-xb[0]),2)+powf((ym-yb[0]),2));
//      theta[0] = atan2f((ym-yb[0]),(xm-xb[0]));
//      zeta = (powf(l3,2) - powf(l1,2) -powf(l2,2))/(2*l1*l2);
//      zeta = limiter(zeta);
//      phid[0] = acosf(zeta);
//      psi[0] = atan2f(l2*sinf(phid[0]),(l1+l2*cosf(phid[0])));
//      q1 = theta[0] + psi[0];
//      q2 = q1 - phid[0];
//
//  }
//  //UARTprintf("IK end\n");
//}
//
//#ifdef DEBUG
//  void
//__error__(char *pcFilename, uint32_t ui32Line)
//{
//}
//#endif
//
//
//
//void SysTickInt(void)
//{
//  uint32_t status=0;
//
//  status = TimerIntStatus(TIMER5_BASE,true);
//  timer_count=1;
//  //timer_count=timer_count+1;
//  //UARTprintf("%u,%u,%u,%u,%u,%u\n",current1,current2,current3,temp1,temp2,temp3);
//  TimerIntClear(TIMER5_BASE,status);
//}
//
////*****************************************************************************
////
//// The UART interrupt handler.
////
////*****************************************************************************
//
//void TimerBegin(){
//
//  //We set the load value so the timer interrupts each 1ms
//  uint32_t Period;
//  //Period = 80000*5; //5ms
//  //Period = 80000*4; //4ms
//  Period = 80000*new_period; //4ms
//
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER5);
//  SysCtlDelay(3);
//  /*
//     Configure the timer as periodic, by omission it's in count down mode.
//     It counts from the load value to 0 and then resets back to the load value.
//REMEMBER: You need to configure the timer before setting the load and match
//*/
//  TimerConfigure(TIMER5_BASE, TIMER_CFG_PERIODIC);
//  TimerLoadSet(TIMER5_BASE, TIMER_A, Period -1);
//
//  TimerIntRegister(TIMER5_BASE, TIMER_A, SysTickInt);
//
//  /*
//     Enable the timeout interrupt. In count down mode it's when the timer reaches
//     0 and resets back to load. In count up mode it's when the timer reaches load
//     and resets back to 0.
//     */
//  TimerIntEnable(TIMER5_BASE, TIMER_TIMA_TIMEOUT);
//
//  TimerEnable(TIMER5_BASE, TIMER_A);
//}
//
//
//void UART0IntHandler(void)
//{
//  int ppp;
//  int jjj;
//
//  int decbase[]={0,1,10,100,1000};
//  uint32_t ui32Status;
//
//  ui32Status = UARTIntStatus(UART0_BASE, true);
//  UARTIntClear(UART0_BASE, ui32Status);
//  valuerx=0;
//  ppp=UARTgets(rxchar,10);
//  //UARTprintf("Int\n");
//  if(rxchar[0]=='d'&& startrx==0) // Turn right
//  {
//      flag_turnr=1;
//      flag_turnl=0;
//      flag_stomp=0;
//      abd_multiplier=0;
//      //flag_FR_turn=1;
//      //flag_BR_turn=1;
//  }
//  if(rxchar[0]=='a'&& startrx==0) // Turn left
//  {
//      flag_turnl=1;
//      flag_turnr=0;
//      flag_stomp=0;
//      abd_multiplier=0;
//      //flag_FL_turn=1;
//      //flag_BL_turn=1;
//  }
//  if(rxchar[0]=='u'&& startrx==0) // Stomp
//  {
//      flag_stomp=1;
//      flag_turnl=0;
//      flag_turnr=0;
//      abd_multiplier=0;
//      flag_cal=0;
//      //flag_FL_turn=1;
//      //flag_BL_turn=1;
//  }
//  if(rxchar[0]=='w'&& startrx==0) // Start walking
//  {
//      startwalking_flag=1;
//      loop_delay=loop_delay-1000;
//  }
//  if(rxchar[0]=='s'&& startrx==0)
//  {
//      loop_delay=loop_delay+1000;
//  }
//  if(rxchar[0]=='n'&& startrx==0)
//  {
//      blxoffset=blxoffset+0.003;
//  }
//  if(rxchar[0]=='N'&& startrx==0)
//  {
//      blxoffset=blxoffset-0.003;
//  }
//  if(rxchar[0]=='m'&& startrx==0)
//  {
//      blyoffset=blyoffset+0.003;
//  }
//  if(rxchar[0]=='M'&& startrx==0)
//  {
//      blyoffset=blyoffset-0.003;
//  }
//
//  if(rxchar[0]=='j'&& startrx==0)
//  {
//      brxoffset=brxoffset+0.003;
//  }
//  if(rxchar[0]=='J'&& startrx==0)
//  {
//      brxoffset=brxoffset-0.003;
//  }
//  if(rxchar[0]=='k'&& startrx==0)
//  {
//      bryoffset=bryoffset+0.003;
//  }
//  if(rxchar[0]=='K'&& startrx==0)
//  {
//      bryoffset=bryoffset-0.003;
//  }
//
//  if(rxchar[0]=='Z'&& startrx==0) // Commence stand-up sequence
//  {
//      //flag_FR_turn=0;
//      //flag_BR_turn=0;
//      //flag_FL_turn=0;
//      //flag_BL_turn=0;
//      flag_turnr=0;
//      flag_turnl=0;
//      flag_stomp=0;
//      begin_flag=1;
//  }
//
//  if(rxchar[0]=='x') // Gait frequency set-point
//  {
//      flag=0;
//      startrx2=1;
//      return;
//  }
//
//  if(rxchar[0]=='X'&& startrx==0)
//  {
//      loop_delay=0;
//  }
//  if(rxchar[0]=='p'&& startrx==0)// Pause
//  {
//      flag=0;
//  }
//  if(rxchar[0]=='P'&& startrx==0) // Resume
//  {
//      flag=1;
//  }
//  if(rxchar[0]=='C'&& startrx==0)
//  {
//      flag_stomp=0;
//  }
//  //flag_cal=0;
//
//
//  // Command to receive the trajectories
//  if(rxchar[0]=='F')
//  {
//      counterrx=0;
//      ////counterrx=1;
//      flag=0;
//      startrx=1;
//      return;
//  }
//
//  // Receive the gait frequency set-point
//  if(startrx2==1)
//  {
//      for(jjj=1;jjj<ppp;jjj++)
//          valuerx=valuerx+decbase[jjj]*(float)(rxchar[ppp-jjj]-48);
//      if(valuerx>250)
//          valuerx=250;
//      if(valuerx<=0)
//          valuerx=0.1;
//      new_period=1/(valuerx)*1000;
//
//      flag=1;
//      startrx2=0;
//  }
//
//  // Receive the trajectory points for all the legs (and spine)
//  if(startrx==1)
//  {
//      for(jjj=1;jjj<ppp;jjj++)
//          valuerx=valuerx+decbase[jjj]*(float)(rxchar[ppp-jjj]-48);
//      if(rxchar[0]=='-')
//          valuerx=valuerx*-1/10000;
//      else
//          valuerx=valuerx/10000;
//
//      if(counterrx<100)
//          FLx[counterrx]=valuerx;
//      else if(counterrx>=100 && counterrx<200)
//          BLx[counterrx-100]=valuerx;
//      else if(counterrx>=200 && counterrx<300)
//          FRx[counterrx-200]=valuerx;
//      else if(counterrx>=300 && counterrx<400)
//          BRx[counterrx-300]=valuerx;
//      else if(counterrx>=400 && counterrx<500)
//          FLy[counterrx-400]=valuerx;
//      else if(counterrx>=500 && counterrx<600)
//          BLy[counterrx-500]=valuerx;
//      else if(counterrx>=600 && counterrx<700)
//          FRy[counterrx-600]=valuerx;
//      else if(counterrx>=700 && counterrx<800)
//          BRy[counterrx-700]=valuerx;
//      else if(counterrx>=800 && counterrx<900)
//          spine13[counterrx-800]=valuerx;
//      else if(counterrx>=900 && counterrx<1000)
//          spine14[counterrx-900]=valuerx;
//      counterrx=counterrx+1;
//      ////counterrx=counterrx+2;
//      ////if(counterrx==801)
//      if(counterrx==1000)
//      {
//          startrx=0;
//          counterrx=0;
//          ////counterrx=1;
//          flag=1;
//      }
//
//  }
//  TimerBegin();
//}
//
////*****************************************************************************
////
//// Send a string to the UART.
////
////*****************************************************************************
//void UARTSend(const uint8_t *pui8Buffer, uint32_t ui32Count)
//{
//  // Loop while there are more characters to send.
//  while(ui32Count--)
//  {
//      // Write the next character to the UART.
//      UARTCharPutNonBlocking(UART0_BASE, *pui8Buffer++);
//  }
//}
//
//
//int main(void)
//{
//
//  //float spine1[]={0.0433,0.0391,0.0347,0.0300,0.0241,0.0171,0.0083,-0.0019,-0.0136,-0.0265,-0.0403,-0.0547,-0.0696,-0.0850,-0.1010,-0.1177,-0.1341,-0.1326,-0.1316,-0.1309,-0.1306,-0.1303,-0.1302,-0.1301,-0.1301,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1300,-0.1296,-0.1288,-0.1265,-0.1234,-0.1196,-0.1149,-0.1093,-0.1029,-0.0958,-0.0883,-0.0805,-0.0724,-0.0643,-0.0567,-0.0494,-0.0421,-0.0347,-0.0278,-0.0214,-0.0149,-0.0081,-0.0013,0.0053,0.0119,0.0187,0.0257,0.0323,0.0386,0.0449,0.0511,0.0570,0.0624,0.0675,0.0727,0.0773,0.0815,0.0861,0.0897,0.0920,0.0934,0.0948,0.0958,0.0959,0.0952,0.0940,0.0931,0.0916,0.0896,0.0872,0.0852,0.0833,0.0808,0.0777,0.0738,0.0693,0.0642,0.0579,0.0528,0.0538,0.0551,0.0543,0.0517,0.0475,0.0433};
//  //float spine2[]={-0.1290,-0.1252,-0.1199,-0.1121,-0.1029,-0.0924,-0.0812,-0.0694,-0.0573,-0.0452,-0.0330,-0.0210,-0.0092,0.0020,0.0128,0.0231,0.0336,0.0574,0.0798,0.1018,0.1234,0.1327,0.1317,0.1310,0.1306,0.1304,0.1302,0.1301,0.1301,0.1301,0.1300,0.1304,0.1304,0.1306,0.1300,0.1299,0.1294,0.1288,0.1276,0.1267,0.1259,0.1245,0.1235,0.1221,0.1220,0.1220,0.1221,0.1222,0.1219,0.1216,0.1204,0.1189,0.1166,0.1138,0.1098,0.1048,0.0992,0.0929,0.0859,0.0783,0.0705,0.0625,0.0544,0.0462,0.0381,0.0303,0.0229,0.0158,0.0091,0.0029,-0.0027,-0.0086,-0.0146,-0.0203,-0.0263,-0.0324,-0.0384,-0.0439,-0.0494,-0.0550,-0.0607,-0.0660,-0.0712,-0.0761,-0.0813,-0.0859,-0.0907,-0.0951,-0.0993,-0.1032,-0.1071,-0.1118,-0.1166,-0.1245,-0.1306,-0.1311,-0.1306,-0.1307,-0.1309,-0.1304,-0.1292};
//
//  int del_pos;
//  int ii=99;
//  int pp=99;
//  float xx,yy;
//  float dt=0.0033;
//  float temp_calc=0;
//  float bly, bry;
//
//  int tempspine1,tempspine2;
//
//  float alpha=0;
//  int delayf=0;
//  //TimerBegin();
//
//  // Set the system clock to 80MHz
//  SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
//          SYSCTL_XTAL_16MHZ);   // 80MHz
//
//
//  // Configure UART1; used for the ICS comm.
//  SysCtlPeripheralReset(SYSCTL_PERIPH_UART1);
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
//  GPIOPinConfigure(GPIO_PB0_U1RX);
//  GPIOPinConfigure(GPIO_PB1_U1TX);
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
//  UARTClockSourceSet(UART1_BASE, UART_CLOCK_PIOSC);
//  GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//  MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
//  MAP_UARTConfigSetExpClk(UART1_BASE, 16000000, 1250000,
//          (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
//           UART_CONFIG_PAR_EVEN));
//  MAP_UARTEnable(UART1_BASE);
//  SysCtlDelay(200);
//
//
//  // Configure the GPIO interface; used in the En pin for ICS
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
//  SysCtlDelay(200);
//  GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_5);  // ICS Enable Pin
//  SysCtlDelay(200);
//  enLow();
//
//  // Configure UART0; used to communicate with the SBC/PC
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
//  GPIOPinConfigure(GPIO_PA0_U0RX);
//  GPIOPinConfigure(GPIO_PA1_U0TX);
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
//  UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
//  GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//  MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
//  MAP_UARTConfigSetExpClk(UART0_BASE, 16000000, 921600,
//          (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
//           UART_CONFIG_PAR_EVEN));
//  //MAP_UARTConfigSetExpClk(UART0_BASE, 16000000, 2000000,
//  //                          (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
//  //                         UART_CONFIG_PAR_EVEN));
//
//  MAP_UARTEnable(UART0_BASE);
//  SysCtlDelay(200);
//  IntEnable(INT_UART0);
//  IntMasterEnable();
//  UARTFIFOLevelSet(UART0_BASE,UART_FIFO_TX4_8,UART_FIFO_RX7_8);
//  UARTIntClear(INT_UART0,UART_INT_RX | UART_INT_RT);
//  UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT);
//  //TimerBegin();
//
//  // Configure GPIO; used for debugging
//  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//  GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3);
//
//  /* SysCtlPeripheralReset(SYSCTL_PERIPH_UART1);
//     InitConsole_B();
//     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//     GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3);
//
//     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
//     GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_5);  // ICS Enable Pin
//     enLow();
//
//     GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1, GPIO_PIN_1);
//     ConfigureUART();
//  //UARTprintf("Hello, world!\n");
//  TimerBegin();*/
//
//
//
//  /*setPos(13,7683);
//    SysCtlDelay(200);
//    setPos(14,7580);
//    SysCtlDelay(200);
//    motor_counter=0;*/
//
//
//  //UARTprintf("Hi\n");
//  //bodyOrientation(-(10)*(3.141/180),(-4)*(3.141/180));
//
//  flag=1;
//  del_pos=100;
//  while(1)
//  {
//      /* if (ii>0)
//         {
//         inverse_stoch(-FLx[ii-1],FLy[ii-1]);
//         q_desired5[ii-1] = q3;
//         q_desired4[ii-1]=  q1+3.14;
//         inverse_stoch(-FLx[(ii+1)%100],FLy[(ii+1)%100]);
//         q_desired5[(ii+1)%100] = q3;
//         q_desired4[(ii+1)%100]=  q1+3.14;
//         inverse_stoch(-FLx[ii],FLy[ii]);
//         q_desired5[ii] = q3;
//         q_desired4[ii]=  q1+3.14;
//         qd_desired5[ii] = (q_desired5[(ii+1)%100] + q_desired5[ii-1] - 2*q_desired5[ii])/(2*dt);
//         qd_desired4[ii] = (q_desired4[(ii+1)%100] + q_desired4[ii-1] - 2*q_desired4[ii])/(2*dt);
//         }
//         else
//         {
//         inverse_stoch(-FLx[ii+1],FLy[ii+1]);
//         q_desired5[ii+1] = q3;
//         q_desired4[ii+1]=  q1+3.14;
//         inverse_stoch(-FLx[ii],FLy[ii]);
//         q_desired5[ii] = q3;
//         q_desired4[ii]=  q1+3.14;
//         qd_desired5[ii] = (q_desired5[ii] - q_desired5[ii+1])/dt;
//         qd_desired4[ii] = (q_desired4[ii] - q_desired4[ii+1])/dt;
//
//         }
//
//         temp_calc = qd_desired5[ii] +alpha*(q_desired5[ii] - q_last5[ii]);
//         q_last5[ii]= dt*temp_calc + q_last5[ii];
//         motor_knee[5] =  reference[5]+ slope*(to_degrees(q_last5[ii]));
//
//         temp_calc = qd_desired4[ii] +alpha*(q_desired4[ii] - q_last4[ii]);
//         q_last4[ii]= dt*temp_calc + q_last4[ii];
//         motor_hip[4] = reference[4] + slope*(to_degrees(q_last4[ii]+3.14));
//         UARTprintf("%d,%d\n",motor_knee[5],motor_hip[4]);*/
//      //UARTprintf("%f,%f\n",qd_desired5[ii],qd_desired4[ii]);
//      pp=0;
//      if(motor_counter==0 && begin_flag==1)
//      {
//          while(pp<100)
//          {
//              xx=0.025;
//              yy=-0.1325+0.08725*cosf(pp*3.14/100);
//
//              if(yy>-0.04 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy Z error\n");
//                  continue;
//              }
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d ",timer_count);
//              motor_knee[5] =  reference[5]+ slope*(to_degrees(q3));
//              motor_hip[4] = reference[4] + slope*(to_degrees(q1+3.14));
//              motor_abd[6] = reference[6];
//
//
//              if(yy>-0.04 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy Z error\n");
//                  continue;
//              }
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d ",timer_count);
//              motor_knee[1] =  reference[1] + slope*(-to_degrees(q3));
//              motor_hip[2] = reference[2] - slope*(to_degrees(q1+3.14));
//              motor_abd[3] = reference[3];
//
//
//              if(yy>-0.04 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy Zerror\n");
//                  continue;
//              }
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d ",timer_count);
//              motor_knee[11] =  reference[11] + slope*(to_degrees(q3));
//              motor_hip[10] = reference[10] + slope*(to_degrees(q1+3.14));
//              motor_abd[12] = reference[12];
//
//
//
//              if(yy>-0.04 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy Zerror\n");
//                  continue;
//              }
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d\n",timer_count);
//              motor_knee[7] =  reference[7] + slope*(-to_degrees(q3));
//              motor_hip[8] = reference[8] - slope*(to_degrees(q1+3.14));
//              motor_abd[9] = reference[9];
//
//
//              actpos[5]=setPos(5,motor_knee[5]);
//              //motor5[ii]=motor_knee[5];
//              SysCtlDelay(del_pos);
//              actpos[4]=setPos(4,motor_hip[4]);
//              //motor4[ii]=motor_hip[4];
//              SysCtlDelay(del_pos);
//              setPos(6,motor_abd[6]);
//              SysCtlDelay(del_pos);
//              current[4]=getCur(4);
//              SysCtlDelay(del_pos);
//              current[5]=getCur(5);
//
//              SysCtlDelay(del_pos);
//
//              actpos[1]=setPos(1,motor_knee[1]);
//              //motor1[ii]=motor_knee[1];
//              SysCtlDelay(del_pos);
//              actpos[2]=setPos(2,motor_hip[2]);
//              //motor2[ii]=motor_hip[2];
//              SysCtlDelay(del_pos);
//              setPos(3,motor_abd[3]);
//              SysCtlDelay(del_pos);
//              current[1]=getCur(1);
//              SysCtlDelay(del_pos);
//              current[2]=getCur(2);
//              SysCtlDelay(del_pos);
//
//              actpos[7]=setPos(7,motor_knee[7]);
//              //motor7[ii]=motor_knee[7];
//              SysCtlDelay(del_pos);
//              actpos[8]=setPos(8,motor_hip[8]);
//              //motor8[ii]=motor_hip[8];
//              SysCtlDelay(del_pos);
//              setPos(9,motor_abd[9]);
//              SysCtlDelay(del_pos);
//              current[7]=getCur(7);
//              SysCtlDelay(del_pos);
//              current[8]=getCur(8);
//              SysCtlDelay(del_pos);
//
//              actpos[11]=setPos(11,motor_knee[11]);
//              //motor11[ii]=motor_knee[11];
//              SysCtlDelay(del_pos);
//              actpos[10]=setPos(10,motor_hip[10]);
//              //motor10[ii]=motor_hip[10];
//              SysCtlDelay(del_pos);
//              setPos(12,motor_abd[12]);
//              SysCtlDelay(del_pos);
//              current[10]=getCur(10);
//              SysCtlDelay(del_pos);
//              current[11]=getCur(11);
//              SysCtlDelay(del_pos);
//
//              pp=pp+1;
//              SysCtlDelay(200000);
//          }
//          pp=99;
//          begin_flag=0;
//          startwalking_flag=0;
//          flag_cal=1;
//          BezierUpdate(action1);
//      }
//
//      if(motor_counter!=0 && startwalking_flag==1){
//        float xy[4] = {0};
//        BezierInference(theta, xy);
//        if(theta > 2*PI)
//        {
//            theta =0.0;
//        }
//        theta = theta + (float)PI/100;
//          if(flag==1)
//          {
//              TimerBegin();
//              GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1, GPIO_PIN_1);
//              //
//              begin_flag=0;
//
//
//              if(flag_cal==1)
//              {
//                  xx=0;
//                  yy=-0.22+flyoffset;
//              }
//              else if(flag_turnl==1)
//              {
//                  xx=0;
//                  yy=FLyt[ii]-0.005;//+0.005;
//                  abd_multiplier=abd_multiplier+0.005;
//                  if(abd_multiplier>0.5)
//                      abd_multiplier=0.5;
//              }
//              else if(flag_turnr==1)
//              {
//                  xx=0;
//                  yy=FRyt[ii]+0.005;//+0.005;
//                  abd_multiplier=abd_multiplier+0.005;
//                  if(abd_multiplier>0.3)
//                      abd_multiplier=0.3;
//              }
//              else if(flag_stomp==1)
//              {
//                  xx=0;
//                  yy=FLyt[ii]+flyoffset;//+0.005;
//                  abd_multiplier=0;
//              }
//              else
//              {
//                  xx=xy[0];
//                  yy=xy[1]+flyoffset;
//                  abd_multiplier=abd_multiplier-0.005;
//                  if(abd_multiplier<0)
//                      abd_multiplier=0;
//              }
//
//
//
//              if(yy>-0.145 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy error\n");
//                  continue;
//              }
//              else if(xx>(-1*(yy+0.01276)/(1.9737)) || xx<((yy+0.01276)/(1.9737)))
//              {
//                  flag=0;
//                  UARTprintf("xx error\n");
//                  continue;
//              }
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d ",timer_count);
//              motor_knee[5] =  reference[5]+ slope*(to_degrees(q3));
//              motor_hip[4] = reference[4] + slope*(to_degrees(q1+3.14));
//              if(flag_turnl==1)
//                  motor_abd[6] = (int)(reference[6]+(1698.5)*abduction6t[ii]*abd_multiplier);
//              else if(flag_turnr==1)
//                  motor_abd[6] = reference[6];
//              else
//                  motor_abd[6] = reference[6];
//              //motor5[ii]=motor_knee[5];
//              //motor4[ii]=motor_hip[4];
//              //UARTprintf("%d,%d,",motor_knee[5],motor_hip[4]);
//
//              if(flag_cal==1)
//              {
//                  xx=0;
//                  yy=-0.22+fryoffset;
//              }
//              else if(flag_turnl==1)
//              {
//                  xx=0;
//                  yy=FRyt[ii]+0.005;//+0.005;
//              }
//              else if(flag_turnr==1)
//              {
//                  xx=0;
//                  yy=FLyt[ii];//+0.005;//+0.005;
//              }
//              else if(flag_stomp==1)
//              {
//                  xx=0;
//                  yy=FRyt[ii]+fryoffset;//+0.005;
//                  abd_multiplier=0;
//              }
//              else
//              {
//                  xx=xy[2];
//                  yy=xy[3]+fryoffset;  //-0.002;
//              }
//
//
//              if(yy>-0.145 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy error\n");
//                  continue;
//              }
//              else if(xx>(-1*(yy+0.01276)/(1.9737)) || xx<((yy+0.01276)/(1.9737)))
//              {
//                  flag=0;
//                  UARTprintf("xx error\n");
//                  continue;
//              }
//
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d ",timer_count);
//              motor_knee[1] =  reference[1] + slope*(-to_degrees(q3));
//              motor_hip[2] = reference[2] - slope*(to_degrees(q1+3.14));
//              if(flag_turnl==1)
//                  motor_abd[3] = reference[3];
//              else if(flag_turnr==1)
//                  motor_abd[3] = (int)(reference[3]-(1698.5)*abduction6t[ii]*abd_multiplier);
//              else
//                  motor_abd[3] = reference[3];
//              //motor1[ii]=motor_knee[1];
//              //motor2[ii]=motor_hip[2];
//              //UARTprintf("%d,%d,",motor_knee[1],motor_hip[2]);
//              /*if(flag_BL_turn==0)
//                xx=-BLx[ii]+blxoffset;
//                else
//                xx=0;
//                yy=BLy[ii]+blyoffset;//-0.005;//-0.06;*/
//
//
//              if(flag_cal==1)
//              {
//                  xx=0;
//                  yy=-0.22+blyoffset;
//              }
//              else if(flag_turnl==1)
//              {
//                  xx=-0.01;//-0.013;
//                  yy=BLyt[ii]-0.005;//+0.005;
//              }
//              else if(flag_turnr==1)
//              {
//                  xx=-0.013;//-0.013;
//                  yy=BRyt[ii]+bryoffset;//+0.005;
//              }
//              else if(flag_stomp==1)
//              {
//                  xx=0; // TODO: Add this if necessary -0.01;
//                  yy=BLyt[ii]+blyoffset;//+0.005;
//                  abd_multiplier=0;
//              }
//              else
//              {
//                  xx=xy[2];
//                  yy=xy[3]+blyoffset;
//              }
//
//
//              if(yy>-0.145 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy error\n");
//                  continue;
//              }
//              else if(xx>(-1*(yy+0.01276)/(1.9737)) || xx<((yy+0.01276)/(1.9737)))
//              {
//                  flag=0;
//                  UARTprintf("xx error\n");
//                  continue;
//              }
//
//
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d ",timer_count);
//              motor_knee[11] =  reference[11] + slope*(to_degrees(q3));
//              motor_hip[10] = reference[10] + slope*(to_degrees(q1+3.14));
//              motor_abd[12] = reference[12];
//              if(flag_turnl==1)
//                  motor_abd[12] = reference[12];
//              else if(flag_turnr==1)
//                  motor_abd[12] = (int)(reference[12]+(1698.5)*abduction9t[ii]*abd_multiplier*2);
//              else
//                  motor_abd[12] = reference[12];
//              //motor11[ii]=motor_knee[11];
//              //motor10[ii]=motor_hip[10];
//              //UARTprintf("%d,%d,",motor_knee[11],motor_hip[10]);
//              /*if(flag_BR_turn==0)
//                xx=-BRx[ii]+brxoffset;
//                else
//                xx=0;
//                yy=BRy[ii]+bryoffset;//-0.06;*/
//              //yy=-0.2349+bryoffset;//-0.06;
//
//
//              if(flag_cal==1)
//              {
//                  xx=0;
//                  yy=-0.22+bryoffset;
//              }
//              else if(flag_turnl==1)
//              {
//                  xx=-0.01;//-0.013;
//                  yy=BRyt[ii]-0.005;//+0.005;
//              }
//              else if(flag_turnr==1)
//              {
//                  xx=-0.013;//-0.013;
//                  yy=BLyt[ii]-0.005;//+0.005;
//              }
//              else if(flag_stomp==1)
//              {
//                  xx=0; // TODO: Add this if necessary -0.01;
//                  yy=BRyt[ii]+bryoffset;//+0.005;
//                  abd_multiplier=0;
//              }
//              else
//              {
//                  xx=xy[0];
//                  yy=xy[1]+bryoffset;
//              }
//
//
//
//
//              if(yy>-0.145 || yy<-0.245)
//              {
//                  flag=0;
//                  UARTprintf("yy error\n");
//                  continue;
//              }
//
//              else if(xx>(-1*(yy+0.01276)/(1.9737)) || xx<((yy+0.01276)/(1.9737)))
//              {
//                  flag=0;
//                  UARTprintf("xx error\n");
//                  continue;
//              }
//
//
//
//              //timer_count=0;
//              inverse_stoch(xx,yy);
//              //UARTprintf("%d\n",timer_count);
//              motor_knee[7] =  reference[7] + slope*(-to_degrees(q3));
//              motor_hip[8] = reference[8] - slope*(to_degrees(q1+3.14));
//              if(flag_turnl==1)
//                  motor_abd[9] =(int)(reference[9]-(1698.5)*abduction9t[ii]*abd_multiplier);
//              else if (flag_turnr==1)
//                  motor_abd[9] = reference[9];
//              else
//                  motor_abd[9] = reference[9];
//              //motor7[ii]=motor_knee[7];
//              //motor8[ii]=motor_hip[8];
//              //UARTprintf("%d,%d,\n",motor_knee[7],motor_hip[8]);
//              //UARTIntDisable(UART0_BASE, UART_INT_RX | UART_INT_RT);
//              //IntDisable(INT_UART0);
//
//              motor_counter=0;
//              IntDisable(INT_UART0);
//              actpos[5]=setPos(5,motor_knee[5]);
//              //motor5[ii]=motor_knee[5];
//              for(delayf=0;delayf<del_pos;delayf++);
//              actpos[4]=setPos(4,motor_hip[4]);
//              //motor4[ii]=motor_hip[4];
//              for(delayf=0;delayf<del_pos;delayf++);
//              setPos(6, motor_abd[6]);
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[4]=getCur(4);
////                velocity[4]=getSpd(4);
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[5]=getCur(5);
////                velocity[5]=getSpd(5);
//              for(delayf=0;delayf<del_pos;delayf++);
//
//
//              actpos[1]=setPos(1,motor_knee[1]);
//              //motor1[ii]=motor_knee[1];
//              for(delayf=0;delayf<del_pos;delayf++);
//              actpos[2]=setPos(2,motor_hip[2]);
//              //motor2[ii]=motor_hip[2];
//              for(delayf=0;delayf<del_pos;delayf++);
//              setPos(3, motor_abd[3]);
//              //setPos(3,(int)(11433-(1698.5)*abduction3t[ii]));
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[1]=getCur(1);
////                velocity[1]=getSpd(1);
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[2]=getCur(2);
////                velocity[2]=getSpd(2);
//              for(delayf=0;delayf<del_pos;delayf++);
//
//
//              actpos[7]=setPos(7,motor_knee[7]);
//              //motor7[ii]=motor_knee[7];
//              for(delayf=0;delayf<del_pos;delayf++);
//              actpos[8]=setPos(8,motor_hip[8]);
//              //motor8[ii]=motor_hip[8];
//              for(delayf=0;delayf<del_pos;delayf++);
//              setPos(9, motor_abd[9]);
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[7]=getCur(7);
////                velocity[7]=getSpd(7);
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[8]=getCur(8);
////                velocity[8]=getSpd(8);
//              for(delayf=0;delayf<del_pos;delayf++);
//
//
//
//              actpos[11]=setPos(11,motor_knee[11]);
//              //motor11[ii]=motor_knee[11];
//              for(delayf=0;delayf<del_pos;delayf++);
//              actpos[10]=setPos(10,motor_hip[10]);
//              //motor10[ii]=motor_hip[10];
//              for(delayf=0;delayf<del_pos;delayf++);
//              setPos(12, motor_abd[12]);
//              //setPos(12,(int)(6250+(1698.5)*abduction12t[ii]));
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[10]=getCur(10);
////                velocity[10]=getSpd(10);
//              for(delayf=0;delayf<del_pos;delayf++);
//              current[11]=getCur(11);
////                velocity[11]=getSpd(11);
//              for(delayf=0;delayf<del_pos;delayf++);
//
//              /*if(flag_cal==1)
//                {
//                tempspine1=7683;
//                tempspine2=7580;
//                }
//                else
//                {
//                tempspine1=(int)(7683+(3397.027)*spine13[ii]);
//                tempspine2=(int)(7580+(3397.027)*spine14[ii]);
//                }
//                setPos(13,tempspine1);
//                SysCtlDelay(del_pos);
//                current[13]=getCur(13);
//                SysCtlDelay(del_pos);
//                setPos(14,tempspine2);
//                SysCtlDelay(del_pos);
//                current[14]=getCur(14);*/
//
////                UARTprintf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",ii,motor_knee[1],velocity[1],current[1],motor_hip[2],velocity[2],current[2],current[3],current[4],current[5],current[6],motor_knee[7],actpos[7],current[7],motor_hip[8],actpos[8],current[8],current[9],current[10],current[11],current[12],tempspine1,actpos[13],current[13],tempspine2,actpos[14],current[14]);
//                UARTprintf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",ii,actpos[1],current[1],actpos[2],current[2],actpos[4],current[4],actpos[5],current[5],actpos[7],current[7],actpos[8],current[8],actpos[10], current[10], actpos[11],current[11]);
//
//              //UARTprintf("%d\n",motor_counter);
//              bly = 1000*blyoffset;
//              bry = 1000*bryoffset;
////                UARTprintf("Offset:: BLy: %d \t BRy: %d \n", (int) bly, (int) bry);
//              ////ii=ii-2;
//              ii=ii-1;
//              if(ii==-1)
//              {
//                  ii=99;
//              }
//
//          }
//          if(loop_delay<0)
//              loop_delay=0;
//          //SysCtlDelay(10000+loop_delay); //80e6/3 count for 1 sec.... 10000 corresponds to 0.000375 secs
//          //1200*20=0.0009 secs ; total of 1.275msec just through sysctldelay
//          IntEnable(INT_UART0);
//          GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_1, 0);
//          while(timer_count==0);
//          timer_count=0;
//          TimerDisable(TIMER5_BASE, TIMER_A);
//
//          //UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT);
//          //SysCtlDelay(80000);
//      }
//  }
//}
